#include "MG_Driver.h"
#include <stdlib.h>

static MG_t **MotorsArray;  // 电机指针数组
static uint8_t MotorsCount; // 电机数量

// CAN中断回调
void MG_CAN_Callback(CAN_RxHeaderTypeDef *pHeader, uint8_t *pBuf)
{
    for (uint8_t i = 0; i < MotorsCount; i++)
    {
        if (MotorsArray[i]->ID == pHeader->StdId)
        {
            MotorsArray[i]->temperature = (int8_t)pBuf[1];
            MotorsArray[i]->current = (int16_t)(pBuf[3] << 8 | pBuf[2]) / 2000.0f * 32;
            MotorsArray[i]->speed = (int16_t)(pBuf[5] << 8 | pBuf[4]) / 10.0f;
            MotorsArray[i]->encoder = (uint16_t)(pBuf[7] << 8 | pBuf[6]) / 10.0f;
        }
    }
}

void MG_Init(MG_t *motor, uint8_t ID, CAN_HandleTypeDef *can)
{
    motor->can = can;
    MotorsCount++; // 电机数+1
    MotorsArray = (MG_t **)realloc(MotorsArray, MotorsCount * sizeof(MG_t *));
    MotorsArray[MotorsCount - 1] = motor;
    motor->ID = ID + 0x140;
}

// 电机关闭
void MG_Close(MG_t *motor)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x80;

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}

// 电机停止
void MG_Stop(MG_t *motor)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x81;

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}

// 电机运行
void MG_Run(MG_t *motor)
{
    uint8_t cmd[8] = {0};

    cmd[0] = 0x88;

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}

/**
 * 速度闭环
 */
void MG_SetSpeed(MG_t *motor, float speed)
{
    int32_t V = (int32_t)(speed * 1000);
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA2;
    cmd[4] = *(uint8_t *)(&V);
    cmd[5] = *((uint8_t *)(&V) + 1);
    cmd[6] = *((uint8_t *)(&V) + 2);
    cmd[7] = *((uint8_t *)(&V) + 3);

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}

/**
 * 位置闭环控制2
 * 多圈控制
 * 设置转角和电机最大速度
 * angle: 度
 */
void MG_SetAngle(MG_t *motor, float angle, uint16_t maxSpeed)
{
    int32_t theta = (int32_t)(angle * 1000);
    uint16_t speed = maxSpeed * 10;
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA4;
    cmd[2] = *(uint8_t *)(&speed);
    cmd[3] = *((uint8_t *)(&speed) + 1);
    cmd[4] = *(uint8_t *)(&theta);
    cmd[5] = *((uint8_t *)(&theta) + 1);
    cmd[6] = *((uint8_t *)(&theta) + 2);
    cmd[7] = *((uint8_t *)(&theta) + 3);

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}

/**
 * 位置闭环控制4
 * 单圈控制
 * 设置转角和最大速度
 * Dir为0x00表示顺时针, 为0x01表示逆时针
 * 输入范围 0° ~ 359.99°
 */
void MG_SetOneAngle(MG_t *motor, float angle, uint8_t dir, uint16_t maxSpeed)
{
    uint16_t theta = (uint16_t)(angle * 1000);
    uint16_t speed = maxSpeed * 10;
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA6;
    cmd[1] = dir;
    cmd[2] = *(uint8_t *)(&speed);
    cmd[3] = *((uint8_t *)(&speed) + 1);
    cmd[4] = *(uint8_t *)(&theta);
    cmd[5] = *((uint8_t *)(&theta) + 1);

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}

/**
 * 位置闭环控制6
 * 位置增量
 * 设置增加角度和最大速度
 * angle: 度
 */
void MG_SetAngleInc(MG_t *motor, float angle, uint16_t maxSpeed)
{
    int32_t theta = (int32_t)(angle * 1000);
    uint16_t speed = maxSpeed * 10;
    uint8_t cmd[8] = {0};

    cmd[0] = 0xA8;
    cmd[2] = *(uint8_t *)(&speed);
    cmd[3] = *((uint8_t *)(&speed) + 1);
    cmd[4] = *(uint8_t *)(&theta);
    cmd[5] = *((uint8_t *)(&theta) + 1);
    cmd[6] = *((uint8_t *)(&theta) + 2);
    cmd[7] = *((uint8_t *)(&theta) + 3);

    while (HAL_CAN_GetTxMailboxesFreeLevel(motor->can) == 0)
    {
        continue;
    }
    CAN_Transmit(motor->can, motor->ID, cmd);
}
